#include "zuc_nml_safe.hh"

void SafetyFuncDIMap_Update(CMS* cms, SafetyFuncDIMap* x) { cms->update(x->diChnFunc, 8); }

void SafetyFuncDOMap_Update(CMS* cms, SafetyFuncDOMap* x) { cms->update(x->doChnFunc, 8); }

void SafetyZone_Update(CMS* cms, SafeZone* safetyZone)
{
    cms->update(safetyZone->safeZoneMode);
    cms->update(safetyZone->elbow_pos_limit_enable);
    cms->update(safetyZone->wrist_pos_limit_enable);
    for (int i = 0; i < 6; i++)
    {
        cms->update(safetyZone->zone.plane[i].enable);
        cms->update(safetyZone->zone.plane[i].name, 255);
        cms->update(safetyZone->zone.plane[i].safeD);
        cms->update(safetyZone->zone.plane[i].safePoint.point.x);
        cms->update(safetyZone->zone.plane[i].safePoint.point.y);
        cms->update(safetyZone->zone.plane[i].safePoint.point.z);
        cms->update(safetyZone->zone.plane[i].safePoint.rpy.y);
        cms->update(safetyZone->zone.plane[i].safePoint.rpy.p);
        cms->update(safetyZone->zone.plane[i].safePoint.rpy.r);
        for (int j = 0; j < 3; j++)
        {
            cms->update(safetyZone->zone.plane[i].planePoints[j].point.x);
            cms->update(safetyZone->zone.plane[i].planePoints[j].point.y);
            cms->update(safetyZone->zone.plane[i].planePoints[j].point.z);
            cms->update(safetyZone->zone.plane[i].planePoints[j].rpy.y);
            cms->update(safetyZone->zone.plane[i].planePoints[j].rpy.p);
            cms->update(safetyZone->zone.plane[i].planePoints[j].rpy.r);
        }
    }
}

void ZUC_SAFE_STAT::update_param(CMS* cms)
{
    SafetyFuncDIMap_Update(cms, &sFuncDISettings);
    SafetyFuncDOMap_Update(cms, &sFuncDOSettings);
    SafetyZone_Update(cms, &safetyZone);
}

void ZUC_ROBOT_SAFETY::update_param(CMS* cms)
{
    cms->update(custome);
    cms->update(lastCustome);
    cms->update(paramType);
    cms->update(momentumLimit);
    cms->update(tcpVelLimit);
    cms->update(robotPowerLimit);
    cms->update(stoppingDistance);
    cms->update(stoppingTime);
    cms->update(type);
}

void ZUC_SET_ROBOT_SAFETY_CUSTOMIZE ::update_param(CMS* cms) { cms->update(customize_type); }

void ZUC_REDUCE_SAFETY::update_param(CMS* cms)
{
    cms->update(tcp_speed);
    cms->update(elbow_speed);
    cms->update(momentum_limit);
    cms->update(power_limit);
    cms->update(stopping_time);
    cms->update(stopping_dist);
}

void ZUC_DRAG_SPEED_LIMIT::update_param(CMS* cms) { cms->update(drag_tcp_speed_limit); }

static void JointLimit_update(CMS* cms, JointPosLimit* x)
{
    cms->update(x->joint_id);
    cms->update(x->positive_limit);
    cms->update(x->negative_limit);
}

void ZUC_JOINT_SAFETY::update_param(CMS* cms)
{
    cms->update(jointNum);
    cms->update(paramType);
    cms->update(jointVelLimit);
    cms->update(jointErrLimit);
    JointLimit_update(cms, &posLimt);
    cms->update(type);
}

void ZUC_FORCE_LIMIT_VALUE::update_param(CMS* cms) { cms->update(force_limit_value); }

void ZUC_COLLISION_DETECTION_ENABLE::update_param(CMS* cms) { cms->update(collision_detection_enable); }

void ZUC_SET_SAFE_ATTITUDE::update_param(CMS* cms)
{
    cms->update(conicalRpy.y);
    cms->update(conicalRpy.p);
    cms->update(conicalRpy.r);
    cms->update(tilt);
    cms->update(pan);
    cms->update(deviation);
    cms->update(deviationwarn);
    cms->update(openEnable);
    cms->update(autoEnable);
    cms->update(tipChoice);
}

void ZUC_SET_COLLISION_OPTION::update_param(CMS* cms)
{
    cms->update(clsnDetectMethod);          // 0: based on position error, 1: momentum observation
    cms->update(clsnReactTypeWhenProgRun);  // 0: program abort and then rebound, 1: program pause without rebounding
    cms->update(clsnReactionReboudnAng);    // rebounding angle
    cms->update(clsnReactionReboudnLen);    //rebounding length
    cms->update(clsnReactionReboudnJAcc);   //rebounding joint move acc
    cms->update(clsnReactionReboudnJVel);   //rebounding joint move vel
    cms->update(clsnReactionReboudnLAcc);   //rebounding linear move acc
    cms->update(clsnReactionReboudnLVel);   //rebounding linear move vel
}